#include <cfloat>
#include <cmath>
#include <cassert>

//#include "System.h"
#include "Ray.h"
#include "AABB.h"
#include "Matrix33.h"
#include "Plane.h"
#include "Sphere.h"
#include "Mathutil.h"

//#include <GL/glew.h>

using namespace gti;

#define RAY_TRI_EPSILON 0.000001f

Ray::Ray()
{
}

Ray::Ray(const vector3f &src, const vector3f &direction) :
origin(src),
dir(direction)
{
}

void Ray::set(const vector3f &src, const vector3f &direction)
{
	origin = src;
	dir = direction;
}

void Ray::computeRay(const vector3f &src, const vector3f &dst)
{
	origin=src;
	dir=(dst-src);
	dir.normalize();
}

void Ray::getPoint(float t, vector3f& point) const
{
	point = (origin+dir*t);
}

bool Clip (float fDenom, float  fNumer, float & rfT0, float & rfT1)
{
    // Return value is 'true' if line segment intersects the current test
    // plane.  Otherwise 'false' is returned in which case the line segment
    // is entirely clipped.

    if ( fDenom > (float)0.0 )
    {
        if ( fNumer > fDenom*rfT1 )
            return false;
        if ( fNumer > fDenom*rfT0 )
            rfT0 = fNumer/fDenom;
        return true;
    }
    else if ( fDenom < (float)0.0 )
    {
        if ( fNumer > fDenom*rfT0 )
            return false;
        if ( fNumer > fDenom*rfT1 )
            rfT1 = fNumer/fDenom;
        return true;
    }
    else
    {
        return fNumer <= (float)0.0;
    }
}

bool FindIntersection (const vector3f &rkOrigin,
    const vector3f &rkDirection, const vector3f &afExtent, float & rfT0,
    float & rfT1)
{
    float fSaveT0 = rfT0, fSaveT1 = rfT1;

    bool bNotEntirelyClipped =
        Clip(+rkDirection.x,-rkOrigin.x-afExtent[0],rfT0,rfT1) &&
        Clip(-rkDirection.x,+rkOrigin.x-afExtent[0],rfT0,rfT1) &&
        Clip(+rkDirection.y,-rkOrigin.y-afExtent[1],rfT0,rfT1) &&
        Clip(-rkDirection.y,+rkOrigin.y-afExtent[1],rfT0,rfT1) &&
        Clip(+rkDirection.z,-rkOrigin.z-afExtent[2],rfT0,rfT1) &&
        Clip(-rkDirection.z,+rkOrigin.z-afExtent[2],rfT0,rfT1);

    return bNotEntirelyClipped && ( rfT0 != fSaveT0 || rfT1 != fSaveT1 );
}

void Ray::getDistance(const vector3f& point, float& dist) const
{
	vector3f nearest;
	getClosestPoint(point,nearest);
	dist = (point - nearest).length();
}

int Ray::intersect(const AABB &aabb,float &tnear,float &tfar) const
{
//http://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm

	const gti::vector3f& vmin = aabb.center-aabb.halfsize;
	const gti::vector3f& vmax = aabb.center+aabb.halfsize;
	
	tnear=-FLT_MAX;//negative infinity
	tfar=FLT_MAX;//positive infinity

	bool bnear = false;
	bool bfar = false;

	// slabs method
	unsigned int i;
	for (i=0;i<3;i++)
	{
		if (fabsf(dir[i])<1e-06f)// if ray is parallel to ith plane
		{			
			if (origin[i]<vmin[i] || origin[i]>vmax[i])
			{
				// origin is not between the slabs
				return false;
			}
			continue;
		}
		// the ray is not parallel to the plane

		float invDir = 1.0f/dir[i];
		float t1 = (vmin[i]-origin[i])*invDir;
		float t2 = (vmax[i]-origin[i])*invDir;
		if (t1 > t2)//since T1 intersection with near plane
		{
			std::swap(t1,t2);
		}
		if (t1 > tnear)// want largest tnear
		{
			tnear = t1;
			bnear = true;
		}

		if (t2 < tfar)// want smallest tfar
		{
			tfar = t2;
			bfar = true;
		}

		if (tnear > tfar) // box is misseed
		{
			return 0;
		}
		if (tfar < 0)//box is behind the ray
		{
			return 0;
		}
	}//for (i=0;i<3;i++)

	if (bnear && bfar)
		return 2;
	else if (bnear || bfar)
		return 1;

	return 0;

/*
    fT0 = 0.0f;
	fT1 = FLT_MAX;
	int riQuantity = 0;
  // convert ray to box coordinates
    const vector3f& kOrigin = origin - aabb.center;

    bool bIntersects = FindIntersection(kOrigin,dir,aabb.halfsize,fT0,fT1);

    if ( bIntersects )
    {
        if ( fT0 > 0.0f )
        {
			riQuantity=2;
        }
        else  // fT0 == 0
        {
			fT0 = fT1;
			riQuantity=1;
        }
    }

    return riQuantity;
*/
}

// fast ray aabb intersection test
int Ray::intersect(const AABB &aabb) const
{
    float fT0 = 0.0f, fT1 = FLT_MAX;
	int riQuantity = intersect(aabb,fT0,fT1);
	return riQuantity;
}

// precise ray aabb intersection test
int Ray::intersect(const AABB &aabb, vector3f& point0, vector3f& point1) const
{
    float fT0 = 0.0f, fT1 = FLT_MAX;
	int riQuantity = intersect(aabb,fT0,fT1);
	if (riQuantity>0)
	{
		getPoint(fT0,point0);
		if (riQuantity==2)
			getPoint(fT1,point1);
	}

    return riQuantity;
}


bool Ray::intersect (const Plane& plane, float &t) const
{
// Solution: t = -(p dot N + d) / (N dot dir)
	
	const float denum = DotProduct(plane.n,dir);

	// parallel to plane
	//if (denum!=0.0f) // cos 0 = 90
	if (fabsf(denum) < 1e-6) // cos 0 = 90
	{
		return false;
	}

	const float num = plane.getDistance(origin);
	t = -num/denum;
	if (std::isnan(t))
	{
		assert(0 && "ray intersect division by zero");
		return false;//safe release
	}
	return true;
}	 

bool Ray::intersect(const vector3f& vert0, const vector3f& vert1, const vector3f& vert2,
					float& t) const
{
	// every point inside a triangle is given by the explicit formula
	//	t(u,v) = o + td = (1-u-v)v0 + u.v1 + v.v2)

	float  det,inv_det;
	float u,v;
	vector3f pvec,tvec,qvec;

	// find vectors for two edges sharing vert0
	const vector3f& edge1 = vert1 - vert0;
	const vector3f& edge2 =  vert2 - vert0;

	// begin calculating determinant - also used to calculate U parameter
	pvec = CrossProduct(dir, edge2);

	// if determinant is near zero, ray lies in plane of triangle
	det = DotProduct(edge1,pvec);

#ifdef TEST_CULL           // define TEST_CULL if culling is desired
	if (det < RAY_TRI_EPSILON)
		return false;

	// calculate distance from vert0 to ray origin
	tvec = orig - vert0;

	// calculate U parameter and test bounds
	u = DotProduct(tvec, pvec);
	if (u < 0.0f || u > det)
		return false;

	// prepare to test V parameter
	qvec = CrossProduct(tvec,edge1);

	// calculate V parameter and test bounds 
	v = DotProduct(dir,qvec);
	if (v < 0.0f || u + v > det)
		return false;

	// calculate t, scale parameters, ray intersects triangle
	t = DotProduct(edge2,qvec);
	inv_det = 1.0f / det;
	t *= inv_det;
	u *= inv_det;
	v *= inv_det;
#else                    // the non-culling branch
	if (fabsf(det) < RAY_TRI_EPSILON )
		return false;
	inv_det = 1.0f / det;


	// calculate distance from vert0 to ray origin
	tvec = origin - vert0;

	// calculate U parameter and test bounds
	u = DotProduct(tvec,pvec) * inv_det;
	if (u < 0.0f || u > 1.0f)
		return false;

	// prepare to test V parameter 
	qvec = CrossProduct(tvec,edge1);

	// calculate V parameter and test bounds
	v = DotProduct(dir, qvec) * inv_det;
	if (v < 0.0f || u + v > 1.0f)
		return false;

	// calculate t, ray intersects triangle
	t = DotProduct(edge2, qvec) * inv_det;
#endif
// compute the point
//	getPoint(t,point);
	return true;
}

void Ray::render() const
{
#ifdef USE_OPENGL
	glBegin(GL_LINES);
	glVertex3fv(origin - dir*50);
	glVertex3fv(origin + dir*50);
	glEnd();
#endif
}

/*
// segment aabb test is here!!!!!
int AABB::intersect(const vector3f &src, const vector3f &dst, float &fT0, float &fT1) const
{
	int riQuantity=0;
// convert ray to box coordinates
	const vector3f kOrigin = src - center;
	const vector3f kDir = dst-src;

	fT0 = 0.0f;
	fT1 = 1.0f;

  bool bIntersects = FindIntersection(kOrigin,kDir,halfsize,
      fT0,fT1);

  if ( bIntersects )
  {
      if ( fT0 > (float)0.0 )
      {
				if (fT1 < (float)1.0) riQuantity=2;
				else riQuantity=1;
      }
      else  // fT0 == 0
      {
				fT0 = fT1;
				riQuantity=1;
      }
  }

  return riQuantity;
}

int AABB::intersect(const vector3f &src, const vector3f &dst, vector3f &point0, vector3f &point1) const
{
	float fT0 = 0.0f;
	float fT1 = 1.0f;

	int riQuantity = intersect(src,dst,fT0,fT1);
	if (riQuantity>0)
	{
		point0=src+(dst-src)*fT0;
		if (riQuantity==2) point1=src+(dst-src)*fT1;
	}
  return riQuantity;
}
*/



int Ray::intersect(const Sphere& sphere, float& t0, float& t1) const
{
	return intersect(sphere.center,sphere.radius,t0,t1);
}

int Ray::intersect(const gti::vector3f& sphereCenter, float r, float& t0, float& t1) const
{
	// sphere equation (centered at origin) x2+y2+z2=r2
	// ray equation x(t) = p0 + t*dir
	// substitute x(t) into sphere equation
	// solution below:

	// transform ray origin into sphere local coordinates
	vector3f orig = origin-sphereCenter;
	float a = dir.x*dir.x + dir.y*dir.y + dir.z*dir.z;
	float b = 2*orig.x*dir.x + 2*orig.y*dir.y + 2*orig.z*dir.z;
	float c = 
		orig.x*orig.x + orig.y*orig.y + orig.z*orig.z - 
		r*r;

	return QuadraticFormula(a,b,c,t0,t1) ? 2 : 0;
}

// computes the distance from origin the the closest point to point
void Ray::getClosestPoint(const vector3f& point, float& t) const
{
// Solution: project point - p onto direction vector
  //             Q      D
  //    SP x-----x------->
  //        \    |
  //         \   |
  //          \  |
  //           \ |
  //            \|
  //             x P
  //
  // P = argument point, SP = line starting point, D = line direction,
  // Q = point to find.
  //
  // Solved by:
  //                         a∑b
  //             comp_b(a) = ---   , a = P-SP, b = D, comp_b(a) = |Q-SP|
  //                         b.b
  //
  //  ==>   Q = SP + comp_b(a)*D

	t = DotProduct(point - origin,dir);
	// dir is a unit vector
}

void Ray::getClosestPoint(const vector3f& point, vector3f& out) const
{
	float t;getClosestPoint(point,t);
	out = origin + t*dir;
}

#undef RAY_TRI_EPSILON 

